/*
 * Planification avec contraintes séquentielles et dérivateur
 *
 */

#include "planification.h"

//Fonction objectif utilisee pour l'optimisation a l'aide de polynomes de 3eme degree
void objectif(int nparam, int j, double *x, double *fj, void *cd);

//fonctions contraintes utilises pour l'optimisation a l'aide de polynome du 3eme degree
void cntr1();
void gradiant();
void contrainte_init();
void gradiant_contraintes();

////////////////////////////////////////////////////////////////////
// Fonction 		: optimisation
// Description		: Algo d'optimisation permmettant de trouver les 2 coefficients de plus haut degree d'une cubique pour les sorties plates x et y afin de respecter les contraintes :
// 3 modes de fonctionnement :
// 1) Mode contournement d'obstacle
// 2) Mode formation de flotille
// 3) Mode sans amis
////////////////////////////////////////////////////////////////////

int optimisation(double init[4], double fin[3], double previous_polynome[8]) {
	int nparam, nf, nineq, neq, mode, iprint, miter, neqn, nineqn, ncsrl,
			ncsrn, nfsr, inform;
	double bigbnd, eps, epsneq, udelta;
	double *x, *bl, *bu, *f, *g, *lambda;
	double *cd;
	double pas = VALEUR_PAS;
	double c0, c1, c2, c3, d0, d1, d2, d3;

	//Le probleme d'optimisation contient une fonction objectif et des contraintes sous forme d'inegalites non lineaires
	//On recherche une fonction cubique telle qu'elle lie le point de depart au point d'arrive en respectant les contraintes
	//et en minimisant la fonction objectif.

	//definition des variables globales qui seront utilisees dans les fonctions objectifs et contraintes


	//Mode de resolution du probleme (CBA)
	mode = 110; // voir manuel pour plus d'informations

	//Affichage des informations
	iprint =0; //Aucune info n'est affichee

	//Maximum d'iteration autorisee par boucle
	miter = 4000;

	//Joue le role de l'infini
	bigbnd = 1.e10;

	//Critere d'arret du calcul
	eps = 1.e-4;

	//Ecart maximum du respect de la contrainte
	epsneq = 1e-3;

	//Permettrai de s'echapper d'un minimum local
	udelta = 1.e-7; //valeur recommandee par le manuel à 0

	//Nombre de variables libres (dimensions de X)
	nparam = 4; // Les 2 coefficients de plus hauts degres de x et de y

	//Limites inferieures des variables libres
	bl = (double *) calloc(nparam, sizeof(double));

	//Limites superieures des variables libres
	bu = (double *) calloc(nparam, sizeof(double));

	//(INPUT)Conditions initiales et (OUTPUT) Iterate at the end of execution
	x = (double *) calloc(nparam, sizeof(double));
	
	//Les donnees clients exploitables dans les fonctions objectifs et contraintes
	cd = (double *) calloc(8, sizeof(double));
	cd[0] = init[0]; //x
	cd[1] = init[1]; //y
	cd[2] = init[2]; //theta
	cd[3] = init[3]; //v
	cd[4] = fin[0]; //x
	cd[5] = fin[1]; //y
	cd[6] = fin[2]; //v
	

/*
	double tmax = VALEUR_PAS * Nutil;
	bl[0] = bl[2] = (-VMIN - init[3])/(3*tmax*tmax) - AMAX/(6*tmax);
	bu[0] = bu[2] = (VMAX - init[3])/(3*tmax*tmax) - AMAX/(6*tmax);
	
	
	bl[1] = bl[3] = -AMIN/2;
	bu[1] = bu[3] = AMAX/2;
*/

	bl[0] = bl[1] = -bigbnd;
	bu[0] = bu[1] = bigbnd;
	bl[2] = bl[3] = -bigbnd;
	bu[2] = bu[3] = bigbnd;


	//Les 4 coefficients de plus haut degre des cubiques initialise a 0
	//x=at^3 + bt^2 +ct +d
	//y=et^3 + ft^2 +gt +h

	//c,d,g et h sont donnee par les conditions initiales (vitesses et positions initiales pour x et y)
	//x[0]==a
	//x[1]==b
	//x[2]==e
	//x[3]==f


	x[0] = previous_polynome[3];
	x[1] = previous_polynome[2];
	x[2] = previous_polynome[7];
	x[3] = previous_polynome[6];
	
	int N = Nfinal_pred;
		cd[7] = N;

		//Nombre de fonctions objectif
		nf = 1;
		//Nombre d'ensemble de fonctions objectif reliees sequentiellement
		nfsr = 0;
		//Nombre de contraintes d'inequation non linaire
		nineqn = (5 + nb_obstacle + nombre_voisins);
		//Nombre total de contraintes d'inequation (i.e. ineq NL + ineq L)
		nineq = (5 + nb_obstacle + nombre_voisins);
		//Nombre de contraintes d'equation NL
		neqn = 0;
		//Nombre total de contraintes d'equation (i.e. eq NL + eq L)
		neq = 0;
		//Nombre d'ensemble de contraintes non lineaires reliees sequentiellement
		ncsrn = 4 + nb_obstacle + nombre_voisins; // 3 ensembles de contraintes liées
		//Nombre d'ensemble de contraintes lineaires reliees sequentiellement
		ncsrl = 0;

		//nombre de contrainte dans chaque ensemble de contraintes NL reliees sequentiellement
		//mesh_pts peut aussi contenir le nombre de fonction obj dans chaque ensemble de fonction objectif
		//et le nombre de contrainte dans chaque ensemble de contrainte L reliees sequentiellement
		// taille du tableau de meshpoints
		int taille_mesh = max(1, nfsr + ncsrn + ncsrl);
		int *mesh_pts = calloc(taille_mesh, sizeof(int));
		int i = 0;
		for (i = 0; i < nfsr; i++) {
			mesh_pts[i] = Nutil;
		}
		for (i = nfsr; i < nfsr + ncsrn; i++) {
			mesh_pts[i] = N;
		}
		for (i = nfsr + ncsrn; i < nfsr + ncsrn + ncsrl; i++) {
			mesh_pts[i] = N;
		}

		//Valeur de la fonction objectif a la fin de l'execution
		int taille = 0;
		for (i = 0; i < nfsr; i++) {
			taille += mesh_pts[i];
		}
		int dimf = max(1, nf - nfsr + taille);
		f = (double *) calloc(dimf, sizeof(double));

		//Valeur des contraintes pour x a la fin de l'execution
		int taille1 = 0;
		for (i = nfsr; i < nfsr + ncsrn; i++) {
			taille1 += mesh_pts[i];
		}
		int taille2 = 0;
		for (i = nfsr + ncsrn; i < nfsr + ncsrn + ncsrl ; i++) {
			taille2 += mesh_pts[i];
		}
		//		fprintf(stderr, "%d %d %d %d %d\n", nineq, neq, ncsrl, ncsrn, taille1);
		int dimg = max(1, nineq + neq - (ncsrl + ncsrn) + taille1 + taille2);
		g = (double *) calloc(dimg, sizeof(double));

		//Values of the Lagrange multipliers at x at the end of execution
		lambda = (double *) calloc(nparam + dimf + dimg, sizeof(double));

		// grcnfd gradiant_contraintes
		//grobfd gradiant
		// appel de la routine
		
		cfsqp(nparam, nf, nfsr, nineqn, nineq, neqn, neq, ncsrl, ncsrn,
				mesh_pts, mode, iprint, miter, &inform, bigbnd, eps, epsneq,
				udelta, bl, bu, x, f, g, lambda, objectif, cntr1, grobfd,
				grcnfd, cd);
		/*
		cfsqp(nparam, nf, nfsr, nineqn, nineq, neqn, neq, ncsrl, ncsrn,
				mesh_pts, mode, iprint, miter, &inform, bigbnd, eps, epsneq,
				udelta, bl, bu, x, f, g, lambda, objectif, cntr1, gradiant,
				gradiant_contraintes, cd);
				*/

		free(mesh_pts);
		free(f);
		free(g);
		free(lambda);

	free(bl);
	free(bu);

	pas = VALEUR_PAS;

	///////////Calcul des trajectoires de x et de y pour les coefficients trouve par CFSQP/////////

	//coefficient de la trajectoire de x
	//position initial
	c0 = init[0]; //x
	//vitesse initiale
	c1 = init[3] * cos(init[2]);
	//Coefficient de plus haut degree trouve par la routine d'optimisation
	c2 = x[1];
	c3 = x[0];

	//coefficient de la trajectoire de y
	//position initiale
	d0 = init[1];
	//vitesse initiale
	d1 = init[3] * sin(init[2]);
	//Coefficient de plus haut degre trouve par la routine d'optimisation
	d2 = x[3];
	d3 = x[2];

	planif[0] = c0;
	planif[1] = c1;
	planif[2] = c2;
	planif[3] = c3;
	planif[4] = d0;
	planif[5] = d1;
	planif[6] = d2;
	planif[7] = d3;

	free(x);
	free(cd);
	return inform;
}

////////////////////////////////////////////////////////////////////
// Fonction 		: objectif
// Description		: fonction objectif pour l'optimisation des cubiques
// elle renvoie la valeur de la distance entre le point final donne par les cubiques
// dont les coefficients sont modifiee par la routine CFSQPP et le point a atteindre ainsi que la distance séparant la trajectoire des points de controle
////////////////////////////////////////////////////////////////////
void objectif(int nparam, int j, double *x, double *fj, void *cdv) {

	double* cd = (double*) cdv;
	double pas = VALEUR_PAS;
	//recuperation de donnees transmises par le client data (cd)
	double x0 = cd[0];
	double y0 = cd[1];
	double dx0 = cd[3] * cos(cd[2]);//==init.v * cos(theta)
	double dy0 = cd[3] * sin(cd[2]);//==init.v * sin(theta)
	double c0, c1, c2, c3, d0, d1, d2, d3;

	//Rappel:
	//cd[4]==fin.x;
	//cd[5]==fin.y;
	//cd[0]==x0==c0==init.x
	//Le vecteur x correspond aux valeurs a optimiser pour le probleme


	// POLYNOME EN X
	c0 = x0; //position initiale
	c1 = dx0; //vitesse initiale
	c2 = x[1]; //coefficient optimisé
	c3 = x[0]; //coefficient optimisé

	//POLYNOME EN Y
	d0 = y0; //position initiale
	d1 = dy0; //vitesse initiale
	d2 = x[3]; //coefficient optimisé
	d3 = x[2]; //coefficient optimisé


	double inter1, inter2;
	double tmax = pas * (Nutil);
	double resultat = 0;
	// On cheche maintenant à rapporcher le robot de son objectif
	if (j == 1) {
		//Calcul du point x le plus loin dans le temps pour le nombre d'iteration choisi et pour les coefficients iteres jusqu'a present
		inter1 = c3 * tmax * tmax * tmax + c2 * tmax * tmax + c1 * tmax + c0;

		//Calcul du point y le plus loin dans le temps pour le nombre d'iteration choisi et pour les coefficients iteres jusqu'a present
		inter2 = d3 * tmax * tmax * tmax + d2 * tmax * tmax + d1 * tmax + d0;

		//Calcul de la distance au point final (on ne prend pas la racine, ce qui revient au meme mais fait gagner du temps)
		//Cette distance est la fonction cout. (cd[4] et cd [5] correspondent aux x et y final)
		resultat += sqrt((cd[4] - inter1) * (cd[4] - inter1) + (cd[5] - inter2)
				* (cd[5] - inter2));
		*fj = resultat;
	}
	return;
}

////////////////////////////////////////////////////////////////////
// Fonction 		: gradiant
// Description		: gradiant de objectif
// renvoie les derivées partielles de la fonction objectif
////////////////////////////////////////////////////////////////////
void gradiant(int nparam, int j, double *x, double *gradf, void(*obj)(int, int,
		double *, double *, void *), double *cd) {
	double pas = VALEUR_PAS;
	double x0 = cd[0];
	double y0 = cd[1];
	double dx0 = cd[3] * cos(cd[2]);//==init.v * cos(theta)
	double dy0 = cd[3] * sin(cd[2]);//==init.v * sin(theta)
	double c0, c1, c2, c3, d0, d1, d2, d3;

	//Calcul des differents coefficients pour les cubiques
	//Rappel:
	//cd[4]==fin.x;
	//cd[5]==fin.y;
	//cd[0]==x0==c0==init.x
	//Le vecteur x correspond aux valeurs a optimiser pour le probleme

	// POLYNOME EN X
	c0 = x0; //position initiale
	c1 = dx0; //vitesse initiale
	c2 = x[1]; //coefficient optimisé
	c3 = x[0]; //coefficient optimisé

	//POLYNOME EN Y
	d0 = y0; //position initiale
	d1 = dy0; //vitesse initiale
	d2 = x[3]; //coefficient optimisé
	d3 = x[2]; //coefficient optimisé

	double tmax = pas * (Nutil);
	double inter1, inter2;

	if (j == 1) {
		//Calcul du point x le plus loin dans le temps pour le nombre d'iteration choisi et pour les coefficients iteres jusqu'a present
		inter1 = c3 * tmax * tmax * tmax + c2 * tmax * tmax + c1 * tmax + c0;

		//Calcul du point y le plus loin dans le temps pour le nombre d'iteration choisi et pour les coefficients iteres jusqu'a present
		inter2 = d3 * tmax * tmax * tmax + d2 * tmax * tmax + d1 * tmax + d0;

		double derx0 = 2* x[0] * tmax * tmax * tmax * tmax * tmax * tmax + 2*
				tmax * tmax * tmax * tmax * tmax * x[1] + (-2* cd [4] * tmax
				* tmax * tmax + 2* c0 * tmax * tmax * tmax + 2 * c1 * tmax
				* tmax * tmax * tmax);
		double derx1 = 2* x [1] * tmax * tmax * tmax * tmax + 2* tmax * tmax
				* tmax * tmax * tmax * x[0] + (-2* cd [4] * tmax * tmax + 2
				* c0 * tmax * tmax + 2 * c1 * tmax * tmax * tmax);
		double derx2 = 2* x [2] * tmax * tmax * tmax * tmax * tmax * tmax + 2*
				tmax * tmax * tmax * tmax * tmax * x[3] + (-2* cd [5] * tmax
				* tmax * tmax + 2* d0 * tmax * tmax * tmax + 2 * d1 * tmax
				* tmax * tmax * tmax);
		double derx3 = 2* x [3] * tmax * tmax * tmax * tmax + 2* tmax * tmax
				* tmax * tmax * tmax * x[2] + (-2* cd [5] * tmax * tmax + 2
				* d0 * tmax * tmax + 2 * d1 * tmax * tmax * tmax);

		if ((cd[4] - inter1) == 0 && (cd[5] - inter2) == 0) {
			gradf[0] = 0;
			gradf[1] = 0;
			gradf[2] = 0;
			gradf[3] = 0;
		} else {
			gradf[0] = (derx0) / (2* sqrt ((cd[4] - inter1) * (cd[4] - inter1)
					+ (cd[5] - inter2) * (cd[5] - inter2)));
			gradf[1] = (derx1) / (2* sqrt ((cd[4] - inter1) * (cd[4] - inter1)
					+ (cd[5] - inter2) * (cd[5] - inter2)));
			gradf[2] = (derx2) / (2* sqrt ((cd[4] - inter1) * (cd[4] - inter1)
					+ (cd[5] - inter2) * (cd[5] - inter2)));
			gradf[3] = (derx3) / (2* sqrt ((cd[4] - inter1) * (cd[4] - inter1)
					+ (cd[5] - inter2) * (cd[5] - inter2)));
		}
	}
	return;
}

////////////////////////////////////////////////////////////////////
// Fonction 		: cntr1
// Description		: contraintes utilisees pour le calcul du polynome de 3eme degres
//			contraintes cinématiques
//			contraintes de non collision avec un obstacle
//			contraintes de non collision entre voisins
////////////////////////////////////////////////////////////////////
void cntr1(int nparam, int j, double *x, double *gj, double *cd) {
	double pas = VALEUR_PAS;
	double x0 = cd[0];
	double y0 = cd[1];
	double dx0 = cd[3] * cos(cd[2]);
	double dy0 = cd[3] * sin(cd[2]);
	double c0, c1, c2, c3, d0, d1, d2, d3;
	
	int Nfinal = cd[7];

	//Calcul des differents coefficients pour les cubiques
	//Rappel:
	//cd[4]==fin.x;
	//cd[5]==fin.y;
	//cd[0]==x0==c0==init.x
	//Le vecteur x correspond aux valeurs a optimiser pour le probleme

	// POLYNOME EN X
	c0 = x0; //position initiale
	c1 = dx0; //vitesse initiale
	c2 = x[1]; //coefficient optimisé
	c3 = x[0]; //coefficient optimisé

	//POLYNOME EN Y
	d0 = y0; //position initiale
	d1 = dy0; //vitesse initiale
	d2 = x[3]; //coefficient optimisé
	d3 = x[2]; //coefficient optimisé
	
	double dec = 1; // nombre de contraintes non linéaires isolées


	// Définition des variables
	int indice_temps, indice_obstacle, indice_voisins;
	double distance_obstacle;
	double t, xr, y, vx, vy, ax, ay;
	double xvois, yvois, distance_robot;
	double vitesse_ang_max = 0;
	double tmax = pas * (Nutil);
	// Fin de définition des variables


	//======================
	// Inegalites cinématiques portant sur la vitesse linéaire, la vitesse angulaire et l'accélération
	//======================
	
	if(j==1){
		vx = 3*c3 * tmax * tmax + 2*c2 * tmax + c1;
		vy = 3*d3 * tmax * tmax + 2*d2 * tmax + d1;
		*gj = sqrt(vx*vx + vy*vy)-cd[6];
		//fprintf(stderr,"vx %f %f %f %f %f\n",vx,c1,c2,c3,*gj);
	}

	if (j >= 1+dec && j <= dec+Nfinal)// Inegalites de vitesse lineaire
	{
		indice_temps = (j - 1) % Nfinal;
		t = pas * indice_temps; //generation du temps
		vx = c1 + 2* c2 * t + 3* c3 * t * t; // generation des vx
		vy = d1 + 2* d2 * t + 3* d3 * t * t; //	generation des vy
		*gj = sqrt(vy * vy + vx * vx) - VMAX; // vitesse - VMAX < 0 <==> vitesse < VMAX <==> contrainte satisfaite
	}
	if (j >= Nfinal + 1 + dec && j <= dec+2* Nfinal ) // Inegalites d'acceleration
	{
		indice_temps = (j - 1) % Nfinal;
		t = pas * indice_temps; //generation du temps
		ax = 2* c2 + 6* c3 * t; // generation des vx
		ay = 2* d2 + 6* d3 * t; //	generation des vy
		*gj = sqrt(ay * ay + ax * ax) - AMAX; // acceleration_max - AMAX < 0 <==> acceleration_max < AMAX <==> contrainte satisfaite
	}

	if (j >= 2* Nfinal + 1 + dec && j <= dec+3* Nfinal ) {// Inegalites de vitesse de rotation
		indice_temps = (j - 1) % Nfinal;
		t = pas * indice_temps; //generation du temps
		vx = c1 + 2* c2 * t + 3* c3 * t * t; // generation des vx
		vy = d1 + 2* d2 * t + 3* d3 * t * t; //	generation des vy
		ax = 2* c2 + 6* c3 * t; // generation des ax
		ay = 2* d2 + 6* d3 * t; //	generation des ay
		if (vx != 0 || vy != 0) //pour eviter les singularites
		{
			vitesse_ang_max = (vx * ay - vy * ax) / (vx * vx + vy * vy); //calcul de la vitesse angulaire max
		} else {
			vitesse_ang_max = 0;
		}

		*gj = vitesse_ang_max - WMAX; // vitesse_ang_max - WMAX < 0 <==> vitesse_ang_max  < WMAX <==> contrainte satisfaite
	}
	
	if (j >= 3* Nfinal + 1 + dec && j <= dec+4* Nfinal ) {// Inegalites de vitesse de rotation
		indice_temps = (j - 1) % Nfinal;
		t = pas * indice_temps; //generation du temps
		vx = c1 + 2* c2 * t + 3* c3 * t * t; // generation des vx
		vy = d1 + 2* d2 * t + 3* d3 * t * t; //	generation des vy
		ax = 2* c2 + 6* c3 * t; // generation des ax
		ay = 2* d2 + 6* d3 * t; //	generation des ay
		if (vx != 0 || vy != 0) //pour eviter les singularites
		{
			vitesse_ang_max = (- vx * ay + vy * ax) / (vx * vx + vy * vy); //calcul de la vitesse angulaire max
		} else {
			vitesse_ang_max = 0;
		}

		*gj = vitesse_ang_max - WMAX; // vitesse_ang_max - WMAX < 0 <==> vitesse_ang_max  < WMAX <==> contrainte satisfaite
	}

	//======================
	// Contraintes d'évitement d'obstacle
	//======================

	if (nb_obstacle > 0) {
		if (j >= (4) * Nfinal + 1 + dec && j <= dec+(5 + nb_obstacle) * Nfinal) // Inegalites de distance a l'obstacle le plus proche
		{
			indice_temps = (j - 1) % Nfinal;
			t = pas * indice_temps; //generation du temps
			xr = c0 + c1 * t + c2 * t * t + c3 * t * t * t; // generation des x
			y = d0 + d1 * t + d2 * t * t + d3 * t * t * t; //	generation des y

			indice_obstacle = (((j - 1) / Nfinal) - 4) % nb_obstacle;
			distance_obstacle = sqrt((xr - coord_obstacle[indice_obstacle][0])
					* (xr - coord_obstacle[indice_obstacle][0]) + (y
					- coord_obstacle[indice_obstacle][1]) * (y
					- coord_obstacle[indice_obstacle][1])); // pour chaque instant calcul de la distance entre le robot et chacun des obstacles
			*gj = -distance_obstacle + dist_obs; //  -distance_obs_min + dist_obs < 0 <==> dist_obs < distance_obs_min <==> contrainte satisfaite
			//			fprintf(stderr,
			//					"[j=%d] t=%f (%f;%f) (%f;%f) d=%f dmin=%f gj=%f\n", j,
			//					t, xr, y, coord_obstacle[indice_obstacle][0],
			//					coord_obstacle[indice_obstacle][1], distance_obstacle,
			//					dist_obs, *gj);
		}
	}
	double tvois;
	if (nombre_voisins > 0) {
		if (j >= (4 + nb_obstacle) * Nfinal + 1 + dec && j <= dec+(5 + nb_obstacle
				+ nombre_voisins) * Nfinal) // Inegalites de distance a l'obstacle le plus proche
		{
			indice_temps = (j - 1) % (Nfinal);
			t = pas * indice_temps; //generation du temps
			//			printf("indice_temps=%d test=%d t=%f\n",indice_temps,(j - 1) % (Nfinal),t);

			xr = c0 + c1 * t + c2 * t * t + c3 * t * t * t; // generation des x
			y = d0 + d1 * t + d2 * t * t + d3 * t * t * t; //	generation des y

			indice_voisins = (((j - 1) / Nfinal) - nb_obstacle - 4)
					% nombre_voisins;

			//			printf("indice_temps=%d test=%d t=%f\n",indice_temps,(j - 1) % (Nfinal),t);
			//			printf("j=%d Nfinal=%d VALEUR_PAS=%f indice_temps=%d indice_voisins=%d \n",j,Nfinal,VALEUR_PAS,indice_temps,indice_voisins);

			double cvois0 = voisins[indice_voisins][0];
			double cvois1 = voisins[indice_voisins][1];
			double cvois2 = voisins[indice_voisins][2];
			double cvois3 = voisins[indice_voisins][3];
			double dvois0 = voisins[indice_voisins][4];
			double dvois1 = voisins[indice_voisins][5];
			double dvois2 = voisins[indice_voisins][6];
			double dvois3 = voisins[indice_voisins][7];

			double temps_depart = temps_voisins[indice_voisins][0];
			//double indice_fin = temps_voisins[indice_voisins][1];
			double base_voisin = temps_voisins[indice_voisins][2];

			tvois = pas * indice_temps + (temps_depart - floor(temps_depart/(Nutil*pas))*(Nutil*pas)); // on calcul la trajectoire du voisin sur la partie restante de sa trajectoire (la partie sur laquelle il peut y avoir collision avec la trajectoire que le robot calcul pour lui)
			
			//printf("indice_temps=%d test=%d tme=%f, tevois = %f\n",indice_temps,(j - 1) % (Nfinal),t,tvois);
			xvois = cvois0 + cvois1 * tvois + cvois2 * tvois * tvois + cvois3
					* tvois * tvois * tvois; // generation des x voisins
			yvois = dvois0 + dvois1 * tvois + dvois2 * tvois * tvois + dvois3
					* tvois * tvois * tvois; //	generation des y voisins


			distance_robot = sqrt((xr - xvois) * (xr - xvois) + (y - yvois)*(y - yvois));
			*gj = -distance_robot + dist_robot_min;
			//fprintf(stderr,"[j=%d] t=%f (%f;%f) t=%f (%f;%f) d=%f dmin=%f gj=%f\n", j,t, xr, y, tvois, xvois, yvois, distance_robot,dist_robot_min, *gj);
		}
	}
	//fprintf(stderr,"j=%d c=%f\n",j,*gj);
}

void gradiant_contraintes(int nparam, int j, double *x, double *gradg,
		void(*constr)(int, int, double *, double *, void *), double *cd) {
	double pas = VALEUR_PAS;
	double x0 = cd[0];
	double y0 = cd[1];
	double dx0 = cd[3] * cos(cd[2]);
	double dy0 = cd[3] * sin(cd[2]);
	double c0, c1, c2, c3, d0, d1, d2, d3;
	
	int Nfinal = cd[7];

	//Calcul des differents coefficients pour les cubiques
	//Rappel:
	//cd[4]==fin.x;
	//cd[5]==fin.y;
	//cd[0]==x0==c0==init.x
	//Le vecteur x correspond aux valeurs a optimiser pour le probleme

	// POLYNOME EN X
	c0 = x0; //position initiale
	c1 = dx0; //vitesse initiale
	c2 = x[1]; //coefficient optimisé
	c3 = x[0]; //coefficient optimisé

	//POLYNOME EN Y
	d0 = y0; //position initiale
	d1 = dy0; //vitesse initiale
	d2 = x[3]; //coefficient optimisé
	d3 = x[2]; //coefficient optimisé
	
	int dec = 1; // nombre de contraintes non linéaires isolées


	// Définition des variables
	int indice_temps, indice_obstacle, indice_voisins;
	double xvois, yvois;
	double t, xr, y, vx, vy, ax, ay;
	// Fin de définition des variables


	//======================
	// Inegalites cinématiques portant sur la vitesse linéaire, la vitesse angulaire et l'accélération
	//======================
	
	// gradiant vitesse finale
	if(j==1){
		indice_temps = Nutil;
		t = pas * indice_temps; //generation du temps
		vx = c1 + 2* c2 * t + 3* c3 * t * t; // generation des vx
		vy = d1 + 2* d2 * t + 3* d3 * t * t; //	generation des vy
		//		*gj = sqrt(vy * vy + vx * vx) - VMAX; // vitesse - VMAX < 0 <==> vitesse < VMAX <==> contrainte satisfaite

		if ((3* t * t * c3 + 2* c2 * t + c1) == 0 && (3* d3 * t * t + 2* d2 * t
				+ d1) == 0) {
			gradg[0] = 0;
			gradg[2] = 0;
		} else {
			gradg[0] = 3* t * t * (3* t * t * c3 + 2* c2 * t + c1) / sqrt((3* t
					* t * c3 + 2* c2 * t + c1) * (3* t * t * c3 + 2* c2 * t
					+ c1) + (3* d3 * t * t + 2* d2 * t + d1) * (3* d3 * t * t
					+ 2* d2 * t + d1));
			gradg[2] = 3* t * t * (3* t * t * d3 + 2* d2 * t + d1) / sqrt((3* t
					* t * d3 + 2* d2 * t + d1) * (3* t * t * d3 + 2* d2 * t
					+ d1) + (3* c3 * t * t + 2* c2 * t + c1) * (3* c3 * t * t
					+ 2* c2 * t + c1));
		}
		if ((2* t * c2 + 3* c3 * t * t + c1) == 0 && (3* d3 * t * t + 2* d2 * t
				+ d1) == 0) {
			gradg[1] = 0;
			gradg[3] = 0;

		} else {
			gradg[1] = 2* t * (2* t * c2 + 3* c3 * t * t + c1) / sqrt((2* t
					* c2 + 3* c3 * t * t + c1) * (2* t * c2 + 3* c3 * t * t
					+ c1) + (3* d3 * t * t + 2* d2 * t + d1) * (3* d3 * t * t
					+ 2* d2 * t + d1));
			gradg[3] = 2* t * (2* t * d2 + 3* d3 * t * t + d1) / sqrt((2* t
					* d2 + 3* d3 * t * t + d1) * (2* t * d2 + 3* d3 * t * t
					+ d1) + (3* c3 * t * t + 2* c2 * t + c1) * (3* c3 * t * t
					+ 2* c2 * t + c1));
		}
	}

	//	printf("indice %d\n",j);
	if (j >= 1+dec && j <= dec+Nfinal)// Inegalites de vitesse lineaire
	{
		indice_temps = (j - 1 - dec) % Nfinal;
		t = pas * indice_temps; //generation du temps
		vx = c1 + 2* c2 * t + 3* c3 * t * t; // generation des vx
		vy = d1 + 2* d2 * t + 3* d3 * t * t; //	generation des vy
		//		*gj = sqrt(vy * vy + vx * vx) - VMAX; // vitesse - VMAX < 0 <==> vitesse < VMAX <==> contrainte satisfaite

		if ((3* t * t * c3 + 2* c2 * t + c1) == 0 && (3* d3 * t * t + 2* d2 * t
				+ d1) == 0) {
			gradg[0] = 0;
			gradg[2] = 0;
		} else {
			gradg[0] = 3* t * t * (3* t * t * c3 + 2* c2 * t + c1) / sqrt((3* t
					* t * c3 + 2* c2 * t + c1) * (3* t * t * c3 + 2* c2 * t
					+ c1) + (3* d3 * t * t + 2* d2 * t + d1) * (3* d3 * t * t
					+ 2* d2 * t + d1));
			gradg[2] = 3* t * t * (3* t * t * d3 + 2* d2 * t + d1) / sqrt((3* t
					* t * d3 + 2* d2 * t + d1) * (3* t * t * d3 + 2* d2 * t
					+ d1) + (3* c3 * t * t + 2* c2 * t + c1) * (3* c3 * t * t
					+ 2* c2 * t + c1));
		}
		if ((2* t * c2 + 3* c3 * t * t + c1) == 0 && (3* d3 * t * t + 2* d2 * t
				+ d1) == 0) {
			gradg[1] = 0;
			gradg[3] = 0;

		} else {
			gradg[1] = 2* t * (2* t * c2 + 3* c3 * t * t + c1) / sqrt((2* t
					* c2 + 3* c3 * t * t + c1) * (2* t * c2 + 3* c3 * t * t
					+ c1) + (3* d3 * t * t + 2* d2 * t + d1) * (3* d3 * t * t
					+ 2* d2 * t + d1));
			gradg[3] = 2* t * (2* t * d2 + 3* d3 * t * t + d1) / sqrt((2* t
					* d2 + 3* d3 * t * t + d1) * (2* t * d2 + 3* d3 * t * t
					+ d1) + (3* c3 * t * t + 2* c2 * t + c1) * (3* c3 * t * t
					+ 2* c2 * t + c1));
		}

	}

	if (j >= Nfinal + 1 + dec && j <= dec+2* Nfinal ) // Inegalites d'acceleration
	{
		indice_temps = (j - 1 - dec) % Nfinal;
		t = pas * indice_temps; //generation du temps
		ax = 2* c2 + 6* c3 * t; // generation des vx
		ay = 2* d2 + 6* d3 * t; //	generation des vy
		//		*gj = sqrt(ay * ay + ax * ax) - AMAX; // acceleration_max - AMAX < 0 <==> acceleration_max < AMAX <==> contrainte satisfaite

		if ((6* t * c3 + 2* c2 ) == 0 && (6* d3 * t + 2* d2 ) == 0) {
			gradg[0] = 0;
			gradg[1] = 0;
			gradg[2] = 0;
			gradg[3] = 0;
		} else {
			gradg[0] = 6* t * (6* t * c3 + 2* c2 ) / sqrt((6* t * c3 + 2* c2 )
					* (6* t * c3 + 2* c2 ) + (6* d3 * t + 2* d2 ) * (6* d3 * t
					+ 2* d2 ));
			gradg[1] = 2 * (2* c2 + 6* c3 * t) / sqrt((2* c2 + 6* c3 * t) * (2*
					c2 + 6* c3 * t) + (6* d3 * t + 2* d2 )
					* (6* d3 * t + 2* d2 ));
			gradg[2] = 6* t * (6* t * d3 + 2* d2 ) / sqrt((6* t * d3 + 2* d2 )
					* (6* t * d3 + 2* d2 ) + (6* c3 * t + 2* c2 ) * (6* c3 * t
					+ 2* c2 ));
			gradg[3] = 6* t * (6* t * d3 + 2* d2 ) / sqrt((6* t * d3 + 2* d2 )
					* (6* t * d3 + 2* d2 ) + (6* c3 * t + 2* c2 ) * (6* c3 * t
					+ 2* c2 ));
		}

	}

	if (j >= 2* Nfinal + 1 + dec && j <= dec + 3* Nfinal ) {// Inegalites de vitesse de rotation
		indice_temps = (j - 1 - dec) % Nfinal;
		t = pas * indice_temps; //generation du temps
		vx = c1 + 2* c2 * t + 3* c3 * t * t; // generation des vx
		vy = d1 + 2* d2 * t + 3* d3 * t * t; //	generation des vy
		ax = 2* c2 + 6* c3 * t; // generation des ax
		ay = 2* d2 + 6* d3 * t; //	generation des ay

		double deter0 = (((3* t * t * c3 + 2* c2 * t + c1) * (3* t * t * c3
				+ 2* c2 * t + c1) + (3* d3 * t * t + 2* d2 * t + d1) * (3* d3
				* t * t + 2* d2 * t + d1)) * ((3* t * t * c3 + 2* c2 * t + c1)
				* (3* t * t * c3 + 2* c2 * t + c1) + (3* d3 * t * t + 2* d2 * t
				+ d1) * (3* d3 * t * t + 2* d2 * t + d1)));

		if (deter0 == 0) {
			gradg[0] = 0;
		} else {
			gradg[0] = (3* t * t * (6* d3 * t + 2* d2 ) - 6* t * (3* d3 * t * t
					+ 2* d2 * t + d1)) / ((3* t * t * c3 + 2* c2 * t + c1)
					* (3* t * t * c3 + 2* c2 * t + c1) + (3* d3 * t * t + 2* d2
					* t + d1) * (3* d3 * t * t + 2* d2 * t + d1)) - 6* t * t
					* (3* t * t * c3 + 2* c2 * t + c1) * ((6* d3 * t + 2* d2 )
					* (3* t * t * c3 + 2* c2 * t + c1) - (3* d3 * t * t + 2* d2
					* t + d1) * (6* t * c3 + 2* c2 )) / deter0;
		}

		double deter1 = (((2* t * c2 + 3* c3 * t * t + c1) * (2* t * c2 + 3* c3
				* t * t + c1) + (3* d3 * t * t + 2* d2 * t + d1) * (3* d3 * t
				* t + 2* d2 * t + d1)) * ((2* t * c2 + 3* c3 * t * t + c1)
				* (2* t * c2 + 3* c3 * t * t + c1) + (3* d3 * t * t + 2* d2 * t
				+ d1) * (3* d3 * t * t + 2* d2 * t + d1)));
		if (deter1 == 0) {
			gradg[1] = 0;
		} else {
			gradg[1] = (2* t * (6* d3 * t + 2* d2 ) - 2* (3* d3*t*t+2*d2*t+d1))/((2*t*c2+3*c3*t*t+c1)*(2*t*c2+3*c3*t*t+c1)+(3*d3*t*t+2*d2*t+d1)*(3*d3*t*t+2*d2*t+d1))-4*t*(2*t*c2+3*c3*t*t+c1)*((6*d3*t+2*d2)*(2*t*c2+3*c3*t*t+c1)-(3*d3*t*t+2*d2*t+d1)*(2*c2+6*c3*t))/deter1;
		}

		double deter2 = (((2* t * c2 + 3* c3 * t * t + c1) * (2* t * c2 + 3* c3
								* t * t + c1) + (3* d3 * t * t + 2* d2 * t + d1) * (3* d3 * t
								* t + 2* d2 * t + d1)) * ((2* t * c2 + 3* c3 * t * t + c1)
						* (2* t * c2 + 3* c3 * t * t + c1) + (3* d3 * t * t + 2* d2 * t
								+ d1) * (3* d3 * t * t + 2* d2 * t + d1)));
		if (deter2 == 0) {
			gradg[2] = 0;
		} else {
			gradg[2]=(6*t*(3*c3*t*t+2*c2*t+c1)-3*t*t*(6*c3*t+2*c2))/((3*t*t*d3+2*d2*t+d1)*(3*t*t*d3+2*d2*t+d1)+(3*c3*t*t+2*c2*t+c1)*(3*c3*t*t+2*c2*t+c1))-6*t*t*(3*t*t*d3+2*d2*t+d1)*((3*c3*t*t+2*c2*t+c1)*(6*t*d3+2*d2)-(6*c3*t+2*c2)*(3*t*t*d3+2*d2*t+d1))/deter2;
		}

		double deter3 = (((2* t * c2 + 3* c3 * t * t + c1) * (2* t * c2 + 3* c3
								* t * t + c1) + (3* d3 * t * t + 2* d2 * t + d1) * (3* d3 * t
								* t + 2* d2 * t + d1)) * ((2* t * c2 + 3* c3 * t * t + c1)
						* (2* t * c2 + 3* c3 * t * t + c1) + (3* d3 * t * t + 2* d2 * t
								+ d1) * (3* d3 * t * t + 2* d2 * t + d1)));
		if (deter3 == 0) {
			gradg[3] = 0;
		} else {
			gradg[3]=(2*(3*c3*t*t+2*c2*t+c1)-2*t*(6*c3*t+2*c2))/((2*t*d2+3*d3*t*t+d1)*(2*t*d2+3*d3*t*t+d1)+(3*c3*t*t+2*c2*t+c1)*(3*c3*t*t+2*c2*t+c1))-4*t*(2*t*d2+3*d3*t*t+d1)*((3*c3*t*t+2*c2*t+c1)*(2*d2+6*d3*t)-(6*c3*t+2*c2)*(2*t*d2+3*d3*t*t+d1))/deter3;
		}
	}
	
		if (j >= 3* Nfinal + 1 + dec && j <= dec + 4* Nfinal ) {// Inegalites de vitesse de rotation
		indice_temps = (j - 1 - dec) % Nfinal;
		t = pas * indice_temps; //generation du temps
		vx = c1 + 2* c2 * t + 3* c3 * t * t; // generation des vx
		vy = d1 + 2* d2 * t + 3* d3 * t * t; //	generation des vy
		ax = 2* c2 + 6* c3 * t; // generation des ax
		ay = 2* d2 + 6* d3 * t; //	generation des ay

		double deter0 = (((3* t * t * c3 + 2* c2 * t + c1) * (3* t * t * c3
				+ 2* c2 * t + c1) + (3* d3 * t * t + 2* d2 * t + d1) * (3* d3
				* t * t + 2* d2 * t + d1)) * ((3* t * t * c3 + 2* c2 * t + c1)
				* (3* t * t * c3 + 2* c2 * t + c1) + (3* d3 * t * t + 2* d2 * t
				+ d1) * (3* d3 * t * t + 2* d2 * t + d1)));

		if (deter0 == 0) {
			gradg[0] = 0;
		} else {
			gradg[0] = -(3* t * t * (6* d3 * t + 2* d2 ) - 6* t * (3* d3 * t * t
					+ 2* d2 * t + d1)) / ((3* t * t * c3 + 2* c2 * t + c1)
					* (3* t * t * c3 + 2* c2 * t + c1) + (3* d3 * t * t + 2* d2
					* t + d1) * (3* d3 * t * t + 2* d2 * t + d1)) - 6* t * t
					* (3* t * t * c3 + 2* c2 * t + c1) * ((6* d3 * t + 2* d2 )
					* (3* t * t * c3 + 2* c2 * t + c1) - (3* d3 * t * t + 2* d2
					* t + d1) * (6* t * c3 + 2* c2 )) / deter0;
		}

		double deter1 = (((2* t * c2 + 3* c3 * t * t + c1) * (2* t * c2 + 3* c3
				* t * t + c1) + (3* d3 * t * t + 2* d2 * t + d1) * (3* d3 * t
				* t + 2* d2 * t + d1)) * ((2* t * c2 + 3* c3 * t * t + c1)
				* (2* t * c2 + 3* c3 * t * t + c1) + (3* d3 * t * t + 2* d2 * t
				+ d1) * (3* d3 * t * t + 2* d2 * t + d1)));
		if (deter1 == 0) {
			gradg[1] = 0;
		} else {
			gradg[1] = -(2* t * (6* d3 * t + 2* d2 ) - 2* (3* d3*t*t+2*d2*t+d1))/((2*t*c2+3*c3*t*t+c1)*(2*t*c2+3*c3*t*t+c1)+(3*d3*t*t+2*d2*t+d1)*(3*d3*t*t+2*d2*t+d1))-4*t*(2*t*c2+3*c3*t*t+c1)*((6*d3*t+2*d2)*(2*t*c2+3*c3*t*t+c1)-(3*d3*t*t+2*d2*t+d1)*(2*c2+6*c3*t))/deter1;
		}

		double deter2 = (((2* t * c2 + 3* c3 * t * t + c1) * (2* t * c2 + 3* c3
								* t * t + c1) + (3* d3 * t * t + 2* d2 * t + d1) * (3* d3 * t
								* t + 2* d2 * t + d1)) * ((2* t * c2 + 3* c3 * t * t + c1)
						* (2* t * c2 + 3* c3 * t * t + c1) + (3* d3 * t * t + 2* d2 * t
								+ d1) * (3* d3 * t * t + 2* d2 * t + d1)));
		if (deter2 == 0) {
			gradg[2] = 0;
		} else {
			gradg[2]=-(6*t*(3*c3*t*t+2*c2*t+c1)-3*t*t*(6*c3*t+2*c2))/((3*t*t*d3+2*d2*t+d1)*(3*t*t*d3+2*d2*t+d1)+(3*c3*t*t+2*c2*t+c1)*(3*c3*t*t+2*c2*t+c1))-6*t*t*(3*t*t*d3+2*d2*t+d1)*((3*c3*t*t+2*c2*t+c1)*(6*t*d3+2*d2)-(6*c3*t+2*c2)*(3*t*t*d3+2*d2*t+d1))/deter2;
		}

		double deter3 = (((2* t * c2 + 3* c3 * t * t + c1) * (2* t * c2 + 3* c3
								* t * t + c1) + (3* d3 * t * t + 2* d2 * t + d1) * (3* d3 * t
								* t + 2* d2 * t + d1)) * ((2* t * c2 + 3* c3 * t * t + c1)
						* (2* t * c2 + 3* c3 * t * t + c1) + (3* d3 * t * t + 2* d2 * t
								+ d1) * (3* d3 * t * t + 2* d2 * t + d1)));
		if (deter3 == 0) {
			gradg[3] = 0;
		} else {
			gradg[3]=-(2*(3*c3*t*t+2*c2*t+c1)-2*t*(6*c3*t+2*c2))/((2*t*d2+3*d3*t*t+d1)*(2*t*d2+3*d3*t*t+d1)+(3*c3*t*t+2*c2*t+c1)*(3*c3*t*t+2*c2*t+c1))-4*t*(2*t*d2+3*d3*t*t+d1)*((3*c3*t*t+2*c2*t+c1)*(2*d2+6*d3*t)-(6*c3*t+2*c2)*(2*t*d2+3*d3*t*t+d1))/deter3;
		}
	}

	//======================
	// Contraintes d'évitement d'obstacle
	//======================

	if (nb_obstacle > 0) {
		if (j >= (4) * Nfinal + 1 + dec && j <= dec + (5 + nb_obstacle) * Nfinal) // Inegalites de distance a l'obstacle le plus proche
		{
			indice_temps = (j - 1 - dec) % Nfinal;
			t = pas * indice_temps; //generation du temps
			xr = c0 + c1 * t + c2 * t * t + c3 * t * t * t; // generation des x
			y = d0 + d1 * t + d2 * t * t + d3 * t * t * t; //	generation des y

			indice_obstacle = (((j - 1 - dec) / Nfinal) - 4) % nb_obstacle;
			double obsv = coord_obstacle[indice_obstacle][0];
			double obsw = coord_obstacle[indice_obstacle][1];

			if((t*t*t*c3+c2*t*t+c1*t-obsv+c0) == 0 && (d3*t*t*t+d2*t*t+d1*t-obsw+d0) == 0)
			{
				gradg[0] = 1.e10;
				gradg[1] = 1.e10;
				gradg[2] = 1.e10;
				gradg[3] = 1.e10;
			}
			else
			{
				gradg[0] = -t*t*t*(t*t*t*c3+c2*t*t+c1*t-obsv+c0)/sqrt( (t*t*t*c3+c2*t*t+c1*t-obsv+c0)*(t*t*t*c3+c2*t*t+c1*t-obsv+c0) +(d3*t*t*t+d2*t*t+d1*t-obsw+d0)*(d3*t*t*t+d2*t*t+d1*t-obsw+d0));
				gradg[1] = -t*t*(t*t*c2+c3*t*t*t+c1*t-obsv+c0)/sqrt( (t*t*c2+c3*t*t*t+c1*t-obsv+c0)*(t*t*c2+c3*t*t*t+c1*t-obsv+c0) +(d3*t*t*t+d2*t*t+d1*t-obsw+d0)*(d3*t*t*t+d2*t*t+d1*t-obsw+d0));
				gradg[2] = -t*t*t*(t*t*t*d3+d2*t*t+d1*t-obsw+d0)/sqrt( (t*t*t*d3+d2*t*t+d1*t-obsw+d0)*(t*t*t*d3+d2*t*t+d1*t-obsw+d0) +(c3*t*t*t+c2*t*t+c1*t-obsv+c0)*(c3*t*t*t+c2*t*t+c1*t-obsv+c0));
				gradg[3] = -t*t*(t*t*d2+d3*t*t*t+d1*t-obsw+d0)/sqrt((t*t*d2+d3*t*t*t+d1*t-obsw+d0)*(t*t*d2+d3*t*t*t+d1*t-obsw+d0)+(c3*t*t*t+c2*t*t+c1*t-obsv+c0)*(c3*t*t*t+c2*t*t+c1*t-obsv+c0));
			}
			//fprintf(stderr,"j=%d p(%f,%f) v(%f,%f) grad[%f,%f,%f,%f]\n",j,xr,y,obsv,obsw,gradg[0],gradg[1],gradg[2],gradg[3]);
		}
	}
	double tvois;
	if (nombre_voisins > 0) {
		if (j >= (4 + nb_obstacle) * Nfinal + 1 + dec && j <= dec+(5 + nb_obstacle
						+ nombre_voisins) * Nfinal) // Inegalites de distance a l'obstacle le plus proche
		{
			//			fprintf(stderr,"j=%d nombre_voisins=%d\n",j,nombre_voisins);
			indice_temps = (j - 1 - dec) % (Nfinal);
			t = pas * indice_temps; //generation du temps
			xr = c0 + c1 * t + c2 * t * t + c3 * t * t * t; // generation des x
			y = d0 + d1 * t + d2 * t * t + d3 * t * t * t; //	generation des y

			indice_voisins = (((j - 1 - dec) / Nfinal) - nb_obstacle - 4)
			% nombre_voisins;

			double cvois0 = voisins[indice_voisins][0];
			double cvois1 = voisins[indice_voisins][1];
			double cvois2 = voisins[indice_voisins][2];
			double cvois3 = voisins[indice_voisins][3];
			double dvois0 = voisins[indice_voisins][4];
			double dvois1 = voisins[indice_voisins][5];
			double dvois2 = voisins[indice_voisins][6];
			double dvois3 = voisins[indice_voisins][7];

			double temps_depart = temps_voisins[indice_voisins][0];
			//			double indice_fin = temps_voisins[indice_voisins][1];
			double base_voisin = temps_voisins[indice_voisins][2];

			tvois = base_voisin * indice_temps + temps_depart; // on calcul la trajectoire du voisin sur la partie restante de sa trajectoire (la partie sur laquelle il 						peut y avoir collision avec la trajectoire que le robot calcul pour lui)
			xvois = cvois0 + cvois1 * tvois + cvois2 * tvois * tvois + cvois3 * tvois * tvois * tvois; // generation des x voisins
			yvois = dvois0 + dvois1 * tvois + dvois2 * tvois * tvois + dvois3 * tvois * tvois * tvois; //	generation des y voisins

			double obsv = xvois;
			double obsw = yvois;

			if((t*t*t*c3+c2*t*t+c1*t-obsv+c0) == 0 && (d3*t*t*t+d2*t*t+d1*t-obsw+d0) == 0)
			{
				gradg[0] = -1.e10;
				gradg[1] = -1.e10;
				gradg[2] = -1.e10;
				gradg[3] = -1.e10;
			}
			else
			{
				gradg[0] = -t*t*t*(t*t*t*c3+c2*t*t+c1*t-obsv+c0)/sqrt( (t*t*t*c3+c2*t*t+c1*t-obsv+c0)*(t*t*t*c3+c2*t*t+c1*t-obsv+c0) +(d3*t*t*t+d2*t*t+d1*t-obsw+d0)*(d3*t*t*t+d2*t*t+d1*t-obsw+d0));
				gradg[1] = -t*t*(t*t*c2+c3*t*t*t+c1*t-obsv+c0)/sqrt( (t*t*c2+c3*t*t*t+c1*t-obsv+c0)*(t*t*c2+c3*t*t*t+c1*t-obsv+c0) +(d3*t*t*t+d2*t*t+d1*t-obsw+d0)*(d3*t*t*t+d2*t*t+d1*t-obsw+d0));
				gradg[2] = -t*t*t*(t*t*t*d3+d2*t*t+d1*t-obsw+d0)/sqrt( (t*t*t*d3+d2*t*t+d1*t-obsw+d0)*(t*t*t*d3+d2*t*t+d1*t-obsw+d0) +(c3*t*t*t+c2*t*t+c1*t-obsv+c0)*(c3*t*t*t+c2*t*t+c1*t-obsv+c0));
				gradg[3] = -t*t*(t*t*d2+d3*t*t*t+d1*t-obsw+d0)/sqrt((t*t*d2+d3*t*t*t+d1*t-obsw+d0)*(t*t*d2+d3*t*t*t+d1*t-obsw+d0)+(c3*t*t*t+c2*t*t+c1*t-obsv+c0)*(c3*t*t*t+c2*t*t+c1*t-obsv+c0));
			}
			fprintf(stderr,"j=%d t=%f p(%f,%f) v(%f,%f) grad[%f,%f,%f,%f]\n",j,t,xr,y,xvois,yvois,gradg[0],gradg[1],gradg[2],gradg[3]);
		}
	}
}

////////////////////////////////////////////////////////////////////
// Fonction 		: cntr1
// Description		: contraintes utilisees pour le calcul du polynome de 3eme degres
//			contraintes cinématiques
//			contraintes de non collision avec un obstacle
//			contraintes de non collision entre voisins
////////////////////////////////////////////////////////////////////
void contrainte_init(int nparam, int j, double *x, double *gj, double *cd) {
	double pas = VALEUR_PAS;
	double x0 = cd[0];
	double y0 = cd[1];
	double dx0 = cd[3] * cos(cd[2]);
	double dy0 = cd[3] * sin(cd[2]);
	double c0, c1, c2, c3, d0, d1, d2, d3;

	//Calcul des differents coefficients pour les cubiques
	//Rappel:
	//cd[4]==fin.x;
	//cd[5]==fin.y;
	//cd[0]==x0==c0==init.x
	//Le vecteur x correspond aux valeurs a optimiser pour le probleme

	// POLYNOME EN X
	c0 = x0; //position initiale
	c1 = dx0; //vitesse initiale
	c2 = x[1]; //coefficient optimisé
	c3 = x[0]; //coefficient optimisé

	//POLYNOME EN Y
	d0 = y0; //position initiale
	d1 = dy0; //vitesse initiale
	d2 = x[3]; //coefficient optimisé
	d3 = x[2]; //coefficient optimisé


	// Définition des variables
	int indice_temps;
	double t, vx, vy, ax, ay;
	double vitesse_max = 0;
	double acceleration_max = 0;
	double vitesse_ang_max = 0;
	// Fin de définition des variables

	switch (j) {

	/*******************************************************************************************************
	 * Inegalites cinématiques portant sur la vitesse linéaire, la vitesse angulaire et l'accélération
	 *******************************************************************************************************/
	case 1: // Inegalites de vitesse lineaire
		for (indice_temps = 0; indice_temps <= Nfinal_pred; indice_temps++) //sur tout l'horizon de calcul
		{
			t = pas * indice_temps; //generation du temps
			vx = c1 + 2* c2 * t + 3* c3 * t * t; // generation des vx
			vy = d1 + 2* d2 * t + 3* d3 * t * t; //	generation des vy
			vitesse_max = max(vitesse_max, sqrt(vy * vy + vx * vx)); //calcul de la vitesse max
		}
		*gj = vitesse_max - VMAX; // vitesse_max - VMAX < 0 <==> vitesse_max < VMAX <==> contrainte satisfaite
		break;

	case 2: // Inegalites d'acceleration
		for (indice_temps = 0; indice_temps <= Nfinal_pred; indice_temps++) //sur tout l'horizon de calcul
		{
			t = pas * indice_temps; //generation du temps
			ax = 2* c2 + 6* c3 * t; // generation des vx
			ay = 2* d2 + 6* d3 * t; //	generation des vy
			acceleration_max = max(acceleration_max, sqrt(ay * ay + ax * ax)); //calcul de l'acceleration max
		}
		*gj = acceleration_max - AMAX; // acceleration_max - AMAX < 0 <==> acceleration_max < AMAX <==> contrainte satisfaite
		break;

	case 3: // Inegalites de vitesse de rotation
		for (indice_temps = 0; indice_temps <= Nfinal_pred; indice_temps++) //sur tout l'horizon de calcul
		{
			t = pas * indice_temps; //generation du temps
			vx = c1 + 2* c2 * t + 3* c3 * t * t; // generation des vx
			vy = d1 + 2* d2 * t + 3* d3 * t * t; //	generation des vy
			ax = 2* c2 + 6* c3 * t; // generation des ax
			ay = 2* d2 + 6* d3 * t; //	generation des ay
			if (vx != 0 && vy != 0) //pour eviter les singularites
			{
				vitesse_ang_max = max(vitesse_ang_max, (vx * ay - vy * ax)
						/ (vx * vx + vy * vy)); //calcul de la vitesse angulaire max
			} else {
				vitesse_ang_max = 0;
			}

		}
		*gj = vitesse_ang_max - WMAX; // vitesse_ang_max - WMAX < 0 <==> vitesse_ang_max  < WMAX <==> contrainte satisfaite
		break;
	}

}
